/*
 * maz_cpnt_g711.c
 *
 *  Created on: 2021年03月13日
 *      Author: wangbing
 *      Email : mz8023yt@163.com
 *      Gitee : https://gitee.com/mazcpnt/maz-g711
 */

#ifdef __cplusplus
extern "C"
{
#endif

#include "maz_cpnt_g711.h"
#include "maz_cpnt_g711_config.h"

//=============================================================================
// G.711 编码
//=============================================================================

/**
 * @brief G.711A编码
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711A数据
 * @param length 输入的PCM数据长度
 */
int MAZ_CPNT_g711a_encode(uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    return MAZ_CPNT_g711_encode(MAZCPNT_E_G711_TYPE_ALAW, ibuf, obuf, length);
}

/**
 * @brief G.711U编码
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711U数据
 * @param length 输入的PCM数据长度
 */
int MAZ_CPNT_g711u_encode(uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    return MAZ_CPNT_g711_encode(MAZCPNT_E_G711_TYPE_ULAW, ibuf, obuf, length);
}

/**
 * @brief G.711编码
 * @param type G.711编码类型
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711数据
 * @param length 输入的PCM数据长度
 */
int MAZ_CPNT_g711_encode(MAZCPNT_E_G711_TYPE type, uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    int i = 0;
    int ret = 0;

    MAZASSERT_RETVAL_NOMSG(MAZCPNT_E_G711_TYPE_ALAW != type && MAZCPNT_E_G711_TYPE_ULAW != type, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(NULL == ibuf, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(0 == length, MAZRET_EINVAL);

    length = length / 2;

    for(i = 0; i < length; i++)
    {
#if (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_LITTLE)
        ret = MAZ_CPNT_g711_encode_one(type, ibuf[2 * i + 1] << 8 | ibuf[2 * i], obuf + i);
#elif (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_BIG)
        ret = MAZ_CPNT_g711_encode_one(type, ibuf[2 * i] << 8 | ibuf[2 * i + 1], obuf + i);
#endif
        MAZASSERT_RETVAL_NOMSG(ret, ret);
    }

    return MAZRET_OK;
}

/**
 * @brief G.711A编码单个采样点
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711A数据
 */
int MAZ_CPNT_g711a_encode_one(int16_t ibuf, uint8_t *obuf)
{
    int16_t pcm13bit = 0;               // PCM 有效值
    int16_t pcm_no_s = 0;               // PCM 绝对值
    uint8_t s = 0;                      // G.711 符号位
    uint8_t eee = 0;                    // G.711 强度位
    uint8_t abcd = 0;                   // G.711 样本位

    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);

    /* 获取有效值 */
    pcm13bit = ibuf >> 3;

    /* 计算符号位 */
    s = 1 - ((pcm13bit & 0x1000) >> 12);
    pcm_no_s = (s == 0) ? ~pcm13bit : pcm13bit & 0xfff;

    /* 计算强度位 */
    if (pcm_no_s >= 0x800)
        eee = 7;
    else if (pcm_no_s >= 0x400)
        eee = 6;
    else if (pcm_no_s >= 0x200)
        eee = 5;
    else if (pcm_no_s >= 0x100)
        eee = 4;
    else if (pcm_no_s >= 0x080)
        eee = 3;
    else if (pcm_no_s >= 0x040)
        eee = 2;
    else if (pcm_no_s >= 0x020)
        eee = 1;
    else
        eee = 0;

    /* 计算样本位 */
    abcd = (pcm_no_s >> (eee ? eee : 1)) & 0xf;

    /* 组合为ALAW码字 */
    *obuf = ((s << 7) | (eee << 4) | abcd) ^ 0x55;

    return MAZRET_OK;
}

/**
 * @brief G.711U编码单个采样点
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711U数据
 */
int MAZ_CPNT_g711u_encode_one(int16_t ibuf, uint8_t *obuf)
{
    int16_t pcm14bit = 0;               // PCM 有效值
    int16_t pcm_no_s = 0;               // PCM 绝对值
    uint8_t s = 0;                      // G.711 符号位
    uint8_t eee = 0;                    // G.711 强度位
    uint8_t abcd = 0;                   // G.711 样本位

    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);

    /* 获取有效值 */
    pcm14bit = ibuf >> 2;

    /* 计算符号位 */
    s = (pcm14bit & 0x2000) >> 13;
    pcm_no_s = (s == 1) ? ~pcm14bit : pcm14bit & 0x1fff;

    /* 计算强度位 */
    if (pcm_no_s >= 0x1000)
        eee = 7;
    else if (pcm_no_s >= 0x800)
        eee = 6;
    else if (pcm_no_s >= 0x400)
        eee = 5;
    else if (pcm_no_s >= 0x200)
        eee = 4;
    else if (pcm_no_s >= 0x100)
        eee = 3;
    else if (pcm_no_s >= 0x080)
        eee = 2;
    else if (pcm_no_s >= 0x040)
        eee = 1;
    else if (pcm_no_s >= 0x020)
        eee = 0;
    else
        eee = 0;                // TODO

    /* 计算样本位 */
    abcd = (pcm_no_s >> (eee + 1)) & 0xf;

    /* 组合为ULAW码字 */
    *obuf = ((s << 7) | (eee << 4) | abcd) ^ 0xff;

    return MAZRET_OK;
}

/**
 * @brief G.711编码单个采样点
 * @param type G.711编码类型
 * @param ibuf 输入的PCM数据
 * @param obuf 输出的G.711数据
 */
int MAZ_CPNT_g711_encode_one(MAZCPNT_E_G711_TYPE type, int16_t ibuf, uint8_t *obuf)
{
    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);

    if(MAZCPNT_E_G711_TYPE_ALAW == type)
    {
        return MAZ_CPNT_g711a_encode_one(ibuf, obuf);
    }
    else if(MAZCPNT_E_G711_TYPE_ULAW == type)
    {
        return MAZ_CPNT_g711u_encode_one(ibuf, obuf);
    }

    return MAZRET_EINVAL;
}

//=============================================================================
// G.711 解码
//=============================================================================

/**
 * @brief G.711A解码
 * @param ibuf 输入的G.711A数据
 * @param obuf 输出的PCM数据
 * @param length 输入的G.711A数据长度
 */
int MAZ_CPNT_g711a_decode(uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    return MAZ_CPNT_g711_decode(MAZCPNT_E_G711_TYPE_ALAW, ibuf, obuf, length);
}

/**
 * @brief G.711U解码
 * @param ibuf 输入的G.711U数据
 * @param obuf 输出的PCM数据
 * @param length 输入的G.711U数据长度
 */
int MAZ_CPNT_g711u_decode(uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    return MAZ_CPNT_g711_decode(MAZCPNT_E_G711_TYPE_ULAW, ibuf, obuf, length);
}

/**
 * @brief G.711解码
 * @param type G.711编码类型
 * @param ibuf 输入的G.711数据
 * @param obuf 输出的PCM数据
 * @param length 输入的G.711数据长度
 */
int MAZ_CPNT_g711_decode(MAZCPNT_E_G711_TYPE type, uint8_t *ibuf, uint8_t *obuf, uint32_t length)
{
    int i = 0;
    int ret = 0;

    MAZASSERT_RETVAL_NOMSG(MAZCPNT_E_G711_TYPE_ALAW != type && MAZCPNT_E_G711_TYPE_ULAW != type, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(NULL == ibuf, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);
    MAZASSERT_RETVAL_NOMSG(0 == length, MAZRET_EINVAL);

    for(i = 0; i < length; i++)
    {
        ret = MAZ_CPNT_g711_decode_one(type, ibuf[i], obuf + i * 2);
        MAZASSERT_RETVAL_NOMSG(ret, ret);
    }

    return MAZRET_OK;
}

/**
 * @brief G.711A解码单个采样点
 * @param ibuf 输入的G.711A数据
 * @param obuf 输出的PCM数据
 */
int MAZ_CPNT_g711a_decode_one(uint8_t ibuf, uint8_t *obuf)
{
    uint8_t s = 0;                      // G.711 符号位
    uint8_t eee = 0;                    // G.711 强度位
    uint8_t abcd = 0;                   // G.711 样本位
    int16_t pcm_no_s = 0;               // PCM 绝对值
    int16_t pcm13bit = 0;               // PCM 有效值
    int16_t pcm16bit = 0;               // PCM 值

    ibuf = ibuf ^ 0x55;

    s = 1 - ((ibuf >> 7) & 0x1);
    eee = (ibuf >> 4) & 0x7;
    abcd = ibuf & 0xf;

    if(0 == eee)
    {
        pcm_no_s = abcd << 1 | 0x1;
    }
    else
    {
        pcm_no_s = 1 << (eee + 4) | 1 << (eee - 1) | abcd << eee;
    }

    pcm13bit = (s == 0) ? pcm_no_s : ~pcm_no_s;
    pcm16bit = pcm13bit << 3;

#if (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_LITTLE)
    *obuf = pcm16bit & 0xff;
    *(obuf + 1) = (pcm16bit >> 8) & 0xff;
#elif (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_BIG)
    *(obuf + 1) = pcm16bit & 0xff;
    *obuf = (pcm16bit >> 8) & 0xff;
#endif

    return 0;
}

/**
 * @brief G.711U解码单个采样点
 * @param ibuf 输入的G.711U数据
 * @param obuf 输出的PCM数据
 */
int MAZ_CPNT_g711u_decode_one(uint8_t ibuf, uint8_t *obuf)
{
    uint8_t s = 0;                      // G.711 符号位
    uint8_t eee = 0;                    // G.711 强度位
    uint8_t abcd = 0;                   // G.711 样本位
    int16_t pcm_no_s = 0;               // PCM 绝对值
    int16_t pcm14bit = 0;               // PCM 有效值
    int16_t pcm16bit = 0;               // PCM 值

    ibuf = ibuf ^ 0xff;

    s = (ibuf >> 7) & 0x1;
    eee = (ibuf >> 4) & 0x7;
    abcd = ibuf & 0xf;

    pcm_no_s = 1 << (eee + 5) | 1 << eee | abcd << (eee + 1);

    pcm14bit = (s == 0) ? pcm_no_s : ~pcm_no_s;
    pcm16bit = pcm14bit << 2;

#if (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_LITTLE)
    *obuf = pcm16bit & 0xff;
    *(obuf + 1) = (pcm16bit >> 8) & 0xff;
#elif (MAZCPNT_G711_ENDIAN == MAZCPNT_G711_ENDIAN_BIG)
    *(obuf + 1) = pcm16bit & 0xff;
    *obuf = (pcm16bit >> 8) & 0xff;
#endif

    return 0;
}

/**
 * @brief G.711解码单个采样点
 * @param type G.711编码类型
 * @param ibuf 输入的G.711数据
 * @param obuf 输出的PCM数据
 */
int MAZ_CPNT_g711_decode_one(MAZCPNT_E_G711_TYPE type, uint8_t ibuf, uint8_t *obuf)
{
    MAZASSERT_RETVAL_NOMSG(NULL == obuf, MAZRET_EINVAL);

    if(MAZCPNT_E_G711_TYPE_ALAW == type)
    {
        return MAZ_CPNT_g711a_decode_one(ibuf, obuf);
    }
    else if(MAZCPNT_E_G711_TYPE_ULAW == type)
    {
        return MAZ_CPNT_g711u_decode_one(ibuf, obuf);
    }

    return MAZRET_EINVAL;
}


#ifdef __cplusplus
}
#endif

